package logic;

import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
import behavior.DriveForward;
import behavior.Emergency;
import behavior.FrontTouch;
import behavior.FrontWall;
import behavior.LeftWall;
import behavior.NoLeftWall;
import behavior.NoRightWall;
import behavior.RightWall;
import behavior.Step;

public class NxtLogic {

	// vars
	private Arbitrator arbitrator;
	private Behavior[] behaviour;

	// constructors
	public NxtLogic() {
		super();
	}

	// methods
	public void run() {
		initArbitrator();
		Motor.A.resetTachoCount();
		Motor.B.resetTachoCount();
		Motor.C.resetTachoCount();
		arbitrator.start();
	}
	
	public void initArbitrator() {
		Behavior b1 = new DriveForward();
		Behavior b2 = new FrontWall(SensorPort.S1);
		Behavior b3 = new LeftWall(SensorPort.S1);
		Behavior b4 = new NoLeftWall(SensorPort.S1);
		Behavior b5 = new RightWall(SensorPort.S1);
		Behavior b6 = new NoRightWall(SensorPort.S1);
		Behavior b7 = new FrontTouch(SensorPort.S4);
		Behavior b8 = new Step(SensorPort.S2);
		Behavior b9 = new Emergency(SensorPort.S1, SensorPort.S2, SensorPort.S3);
		behaviour = new Behavior[] { b1, b2, b3, b4, b5, b6, b7, b8, b9 };
		System.out.println("Behaviors created");

		arbitrator = new Arbitrator(behaviour);
		System.out.println("Arbitrator created");
	}

}
